
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include  <roborts_costmap/Nlayer.h>
#include "punish_layer_setting.pb.h"
#include "punish_layer.h"
//PLUGINLIB_EXPORT_CLASS(punish_layer_namespace::PunishLayer, roborts_costmap::Layer)

using roborts_costmap::LETHAL_OBSTACLE;
using roborts_costmap::NO_INFORMATION;
using roborts_costmap::FREE_SPACE;
 


namespace roborts_costmap{
 
  PunishLayer::PunishLayer(std::string map_name):map_name_(map_name) {   
    flag = 0;
      Derail7 = true;
      Derail8 = true;
      Derail9 = true;
    punish_area_1_map.header.frame_id = "/map";
    punish_area_1_map.point.x;
    punish_area_1_map.point.y;
    punish_area_2_map.header.frame_id = "/map";
    punish_area_2_map.point.x;
    punish_area_2_map.point.y ;
    punish_area_3_map.header.frame_id = "/map";
    punish_area_3_map.point.x;
    punish_area_3_map.point.y;
    punish_area_4_map.header.frame_id = "/map";
    punish_area_4_map.point.x;
    punish_area_4_map.point.y ;
    punish_area_5_map.header.frame_id = "/map";
    punish_area_5_map.point.x;
    punish_area_5_map.point.y;
    punish_area_6_map.header.frame_id = "/map";
    punish_area_6_map.point.x;
    punish_area_6_map.point.y ;
    punish_area_7_map.header.frame_id = "/map";
    punish_area_7_map.point.x;
    punish_area_7_map.point.y;
    punish_area_8_map.header.frame_id = "/map";
    punish_area_8_map.point.x;
    punish_area_8_map.point.y ;
    punish_area_9_map.header.frame_id = "/map";
    punish_area_9_map.point.x;
    punish_area_9_map.point.y ;
    
   if(map_name_ == "global_costmap")
    {
      punish_area_1 = punish_area_1_map;
      punish_area_2 = punish_area_2_map;
      punish_area_3 = punish_area_3_map;
      punish_area_4 = punish_area_4_map;    
      punish_area_5 = punish_area_5_map;
      punish_area_6 = punish_area_6_map;
      punish_area_7 = punish_area_7_map;
      punish_area_8 = punish_area_8_map;
      punish_area_9 = punish_area_9_map;
    }
  }

   int lengtha;
   int lengthb;
   int lengtha1;
   int lengthb1;
   int lengtha2;
   int lengthb2;
   int lengthi;
  void PunishLayer::OnInitialize(){
    ros::NodeHandle nh("~/" + name_),g_nh;
    ParaPunishLayer  para_punishlayer;

    std::string punish_layer = ros::package::getPath("roborts_costmap") + \
      "/config/punish_layer_config.prototxt";
    roborts_common::ReadProtoFromTextFile(punish_layer.c_str(), &para_punishlayer);
    punish_area_1_map.point.x = para_punishlayer.xa() ;
    punish_area_1_map.point.y = para_punishlayer.ya() ;
    punish_area_2_map.point.x = para_punishlayer.xb() ;
    punish_area_2_map.point.y = para_punishlayer.yb() ;
    punish_area_3_map.point.x = para_punishlayer.xc() ;
    punish_area_3_map.point.y = para_punishlayer.yc() ;
    punish_area_4_map.point.x = para_punishlayer.xd() ;
    punish_area_4_map.point.y = para_punishlayer.yd() ;
    punish_area_5_map.point.x = para_punishlayer.xe() ;
    punish_area_5_map.point.y = para_punishlayer.ye() ;
    punish_area_6_map.point.x = para_punishlayer.xf() ;
    punish_area_6_map.point.y = para_punishlayer.yf() ;
    punish_area_7_map.point.x = para_punishlayer.xg() ;
    punish_area_7_map.point.y = para_punishlayer.yg() ;
    punish_area_8_map.point.x = para_punishlayer.xh() ;
    punish_area_8_map.point.y = para_punishlayer.yh() ;
    punish_area_9_map.point.x = para_punishlayer.xi() ;
    punish_area_9_map.point.y = para_punishlayer.yi() ;
    double widtha1 =para_punishlayer.widtha1();
    double widthb1 =para_punishlayer.widthb1();
    double widtha2 =para_punishlayer.widtha2();
    double widthb2 =para_punishlayer.widthb2();
    double widtha =para_punishlayer.widtha();
    double widthb =para_punishlayer.widthb();
    double widthi =para_punishlayer.widthi();


    lengtha = int(10*widtha);
    lengthb = int(10*widthb);
    lengtha1 = int(10*widtha1);
    lengthb1 = int(10*widthb1);
    lengtha2 = int(10*widtha2);
    lengthb2 = int(10*widthb2);
    lengthi = int(10*widthi);
      punish_area_1 = punish_area_1_map;
      punish_area_2 = punish_area_2_map;
      punish_area_3 = punish_area_3_map;
      punish_area_4 = punish_area_4_map;    
      punish_area_5 = punish_area_5_map;
      punish_area_6 = punish_area_6_map;
      punish_area_7 = punish_area_7_map;
      punish_area_8 = punish_area_8_map;
      punish_area_9 = punish_area_9_map;

    is_current_ = true;
    is_enabled_ = true;
    default_value_ = NO_INFORMATION;
    MatchSize();
    nlayer_sub_ = nh.subscribe("/current_buff_open_info", 10,&PunishLayer::NlayerInfoCallback,this);
    tf_->waitForTransform("odom","map",ros::Time::now(),ros::Duration(5.0));
  }

 


  void PunishLayer::NlayerInfoCallback(const roborts_costmap:: Nlayer :: ConstPtr&msg){
      ROS_INFO("Subcribe NLayer Info: derail:%d,%d,%d,%d,%d,%d", msg->derail[0],msg->derail[1],msg->derail[2],msg->derail[3],msg->derail[4],msg->derail[5]);
      if(msg->derail[0] == 1) {
        Derail1 = true;
      }
      else {
        Derail1 = false;
      }

      if(msg->derail[1] == 1) {
        Derail2 = true;
      }
      else if(msg->derail[2] == 0){
        Derail2 = false;
      }

      if(msg->derail[2] == 1) {
        Derail3 = true;
      }
      else if(msg->derail[2] == 0){
        Derail3 = false;
      }

      if(msg->derail[3] == 1) {
        Derail4 = true;
      }
      else if(msg->derail[3] == 0){
        Derail4 = false;
      }

      if(msg->derail[4] == 1) {
        Derail5 = true;
      }
      else if(msg->derail[4] == 0){
        Derail5 = false;
      }

      if(msg->derail[5] == 1) {
        Derail6 = true;
      }
      else if(msg->derail[5] == 0){
        Derail6 = false;
      }
  } 

  void PunishLayer::MatchSize(){

    Costmap2D* master = layered_costmap_->GetCostMap();
    ResizeMap(master->GetSizeXCell(), master->GetSizeYCell(), master->GetResolution(),master->GetOriginX(), master->GetOriginY());
  }

  void PunishLayer::UpdateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y){

    if (!is_enabled_)
      return;
    if (flag == 0){
      flag = 1;
  }
    else{
      return;
    }
    double mark_x = 1;
    double mark_y = 1;
    *min_x = std::min(*min_x, mark_x);
    *min_y = std::min(*min_y, mark_y);
    *max_x = std::max(*max_x, mark_x);
    *max_y = std::max(*max_y, mark_y);
  }
  
  void PunishLayer::UpdateCosts(roborts_costmap::Costmap2D& master_new, int min_i, int min_j, int max_i, int max_j){
    unsigned int mx;
    unsigned int my;
    double mark_x ,mark_y; 
    double base_x,base_y;
    base_x = layered_costmap_->GetCostMap()->GetOriginX();
    base_y = layered_costmap_->GetCostMap()->GetOriginY();
  

    if (!is_enabled_)
    return;
    for (int j = min_j; j < max_j; j++){
      for (int i = min_i; i < max_i; i++){

        if(!Derail1) {
          for(auto cell:punish_cells){
            SetCost(cell.first,cell.second,FREE_SPACE);
          }
        punish_cells.clear();
            mark_x = punish_area_1.point.x -base_x;
            mark_y = punish_area_1.point.y -base_y;
            double markxi=mark_x;
            double markyi=mark_y;
          for(int i=0;i<lengtha;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                  SetCost(mx, my,FREE_SPACE);
              }
            }
          }
        }
        else{
         for(auto cell:punish_cells){
          SetCost(cell.first,cell.second,FREE_SPACE);
         }
        punish_cells.clear();
          mark_x = punish_area_1.point.x -base_x;
          mark_y = punish_area_1.point.y -base_y;
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));
              }
            }
          }
        }
 
        if(!Derail2){
            mark_x = punish_area_2.point.x -base_x;
            mark_y = punish_area_2.point.y -base_y;
            double markxi=mark_x;
            double markyi=mark_y;
            for(int i=0;i<lengtha;i++) { 
              markxi=mark_x+0.05*i;
              for(int j=0;j<lengthb;j++) {
                markyi=mark_y+0.05*j;
                if(World2Map(markxi, markyi, mx, my)){
                  SetCost(mx, my,FREE_SPACE);
                }
              }
            }
          }
        else{
          mark_x = punish_area_2.point.x -base_x;
          mark_y = punish_area_2.point.y -base_y; 
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));
              }
            }
          }
        }
      
        if(!Derail3){
            mark_x = punish_area_3.point.x -base_x;
            mark_y = punish_area_3.point.y -base_y;
            double markxi=mark_x;
            double markyi=mark_y;
            for(int i=0;i<lengtha;i++) { 
              markxi=mark_x+0.05*i;
              for(int j=0;j<lengthb;j++) {
                markyi=mark_y+0.05*j;
                if(World2Map(markxi, markyi, mx, my)){
                  SetCost(mx, my,FREE_SPACE);
                }
              }
            }
          }
        else{
          mark_x = punish_area_3.point.x -base_x;
          mark_y = punish_area_3.point.y -base_y; 
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));
              }
            }
          }
        }

        if(!Derail4){
          mark_x = punish_area_4.point.x -base_x;
          mark_y = punish_area_4.point.y -base_y;
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,FREE_SPACE);
              }
            }
          }
        }
        else{
          mark_x = punish_area_4.point.x -base_x;
          mark_y = punish_area_4.point.y -base_y; 
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));
              }
            }
          }
        }

        if(!Derail5){

          
//std::cout<<"yoyoyo"<<std::endl;
          mark_x = punish_area_5.point.x -base_x;
          mark_y = punish_area_5.point.y -base_y;
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){

                SetCost(mx, my,FREE_SPACE);
              }
            }
          }
        }
        else{
          

          mark_x = punish_area_5.point.x -base_x;
          mark_y = punish_area_5.point.y -base_y; 
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));
              }
            }
          }
        }

        if(!Derail6){
          mark_x = punish_area_6.point.x -base_x;
          mark_y = punish_area_6.point.y -base_y;
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,FREE_SPACE);
              }
            }
          }
        }
        else{
          mark_x = punish_area_6.point.x -base_x;
          mark_y = punish_area_6.point.y -base_y; 
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));

              }
            }
          }
        }

        if(!Derail7){
          mark_x = punish_area_7_map.point.x-base_x ;
          mark_y = punish_area_7_map.point.y -base_y;
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha1;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengtha2;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,FREE_SPACE);
              }
            }
          }
        }
        else{
          mark_x = punish_area_7_map.point.x -base_x;
          mark_y = punish_area_7_map.point.y -base_y; 
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengtha1;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengtha2;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));

              }
            }
          }
        }

        if(!Derail8){
          mark_x = punish_area_8_map.point.x -base_x;
          mark_y = punish_area_8_map.point.y -base_y;
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengthb1;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb2;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,FREE_SPACE);
              }
            }
          }
        }
        else{
          mark_x = punish_area_8_map.point.x -base_x;
          mark_y = punish_area_8_map.point.y -base_y; 
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<lengthb1;i++) { 
            markxi=mark_x+0.05*i;
            for(int j=0;j<lengthb2;j++) {
              markyi=mark_y+0.05*j;
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));

              }
            }
          }
        }

        if(!Derail9){
          mark_x = punish_area_9_map.point.x -base_x;
          mark_y = punish_area_9_map.point.y -base_y;
          double markxi=mark_x;
          double markyi=mark_y;
          for(int i=0;i<=lengthi;i++) { 
              markxi=mark_x+0.05*i;
              markyi=mark_y+0.05*(lengthi-i);
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,FREE_SPACE);
              }
              markxi=mark_x+0.05*i;
              markyi=mark_y-0.05*(lengthi-i);
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,FREE_SPACE);
              }
              markxi=mark_x-0.05*i;
              markyi=mark_y-0.05*(lengthi-i);
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,FREE_SPACE);
              }
              markxi=mark_x-0.05*i;
              markyi=mark_y+0.05*(lengthi-i);
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,FREE_SPACE);
              }
          }
        }
        else{
            mark_x = punish_area_9_map.point.x -base_x;
            mark_y = punish_area_9_map.point.y -base_y;
            double markxi=mark_x;
            double markyi=mark_y;
            for(int i=0;i<lengthi;i++) { 
              markxi=mark_x+0.05*i;
              markyi=mark_y+0.05*(lengthi-i);
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));
              }
              markxi=mark_x+0.05*i;
              markyi=mark_y-0.05*(lengthi-i);
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));
              }
              markxi=mark_x-0.05*i;
              markyi=mark_y-0.05*(lengthi-i);
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));
              }
              markxi=mark_x-0.05*i;
              markyi=mark_y+0.05*(lengthi-i);
              if(World2Map(markxi, markyi, mx, my)){
                SetCost(mx, my,LETHAL_OBSTACLE);
                punish_cells.push_back(std::pair<unsigned int,unsigned int>(mx,my));
              }
          }
        }

        int index = GetIndex(i, j);
        if (costmap_[index] == NO_INFORMATION)
          continue;
        master_new.SetCost(i, j, costmap_[index]); 
      }
    }
  }
} 
// end namespace